#include <opencv2/opencv.hpp>
#include <tld_utils.h>
#include <iostream>
#include <sstream>
#include <TLD.h>
#include <stdio.h>
#include <unistd.h>
using namespace cv;
using namespace std;
//Global variables
Rect box;
bool drawing_box = false;
bool gotBB = false;
bool tl = true;
bool rep = false;
bool fromfile=false;
string video;

void readBB(char* file){
  ifstream bb_file (file);
  string line;
  getline(bb_file,line);
  istringstream linestream(line);
  string x1,y1,x2,y2;
  getline (linestream,x1, ',');
  getline (linestream,y1, ',');
  getline (linestream,x2, ',');
  getline (linestream,y2, ',');
  int x = atoi(x1.c_str());// = (int)file["bb_x"];
  int y = atoi(y1.c_str());// = (int)file["bb_y"];
  int w = atoi(x2.c_str())-x;// = (int)file["bb_w"];
  int h = atoi(y2.c_str())-y;// = (int)file["bb_h"];

  box = Rect(x,y,w,h);
}
//bounding box mouse callback
void mouseHandler(int event, int x, int y, int flags, void *param){
  switch( event ){
  case CV_EVENT_MOUSEMOVE:
    if (drawing_box){
        box.width = x-box.x;
        box.height = y-box.y;
    }
    break;
  case CV_EVENT_LBUTTONDOWN:
    drawing_box = true;
    box = Rect( x, y, 0, 0 );
    break;
  case CV_EVENT_LBUTTONUP:
    drawing_box = false;
    if( box.width < 0 ){
        box.x += box.width;
        box.width *= -1;
    }
    if( box.height < 0 ){
        box.y += box.height;
        box.height *= -1;
    }
    gotBB = true;
    break;
  }
}

void print_help(char** argv){
  printf("use:\n     %s -p /path/parameters.yml\n",argv[0]);
  printf("-s    source video\n-b        bounding box file\n-tl  track and learn\n-r     repeat\n");
}

void read_options(int argc, char** argv,VideoCapture& capture,FileStorage &fs){
  for (int i=0;i<argc;i++){
      if (strcmp(argv[i],"-b")==0){
          if (argc>i){
              readBB(argv[i+1]);
              gotBB = true;
          }
          else
            print_help(argv);
      }
      if (strcmp(argv[i],"-s")==0){
          if (argc>i){
              video = string(argv[i+1]);
              // capture.open(video);
              fromfile = true;
          }
          else
            print_help(argv);

      }
      if (strcmp(argv[i],"-p")==0){
          if (argc>i){
              fs.open(argv[i+1], FileStorage::READ);
          }
          else
            print_help(argv);
      }
      if (strcmp(argv[i],"-no_tl")==0){
          tl = false;
      }
      if (strcmp(argv[i],"-r")==0){
          rep = true;
      }
  }
}

#define H_HEIGHT 2048
#define W_WIDTH 2048

#define SEARCH_H 256
#define SEARCH_W 256

#define MOBAN_H 64
#define MOBAN_W 64

#define S_SCALE 90

#if 1
VideoWriter writer;
VideoWriter writer1;
Mat rotateImg;
Mat rotateImg_temp;
Mat rotateImg_temp_xuan;
BoundingBox xg_box;
Mat current_gray;
TLD tld;
int learn_flag = 0;

void* tracking_learn_process(void *ptr)
{
    while (1)
    {
        if(learn_flag != 1)
        {
            usleep(1000);
            continue;
        }
        else
        {
            tld.learn(current_gray);
            learn_flag = 0;
        }
    }
}


int main(int argc, char * argv[])
{
  VideoCapture capture;
//  capture.open(0);
//  capture.open("./out_src.avi");
  FileStorage fs;
  //Read options
  read_options(argc,argv,capture,fs);
  //Init camera
  // if (!capture.isOpened())
  {
	cout << "capture device failed to open!" << endl;
    // return 1;
  }

  int frame_fps = 27;
  int isColor = 1;
  //要保存的视频 高 和 宽
  int frame_width = W_WIDTH;
  int frame_height = H_HEIGHT;
  writer = VideoWriter("./live1_track.avi", CV_FOURCC('M', 'J', 'P', 'G'), frame_fps, Size(frame_width, frame_height), isColor);
  // writer1 = VideoWriter("./live1_track.avi", CV_FOURCC('M', 'J', 'P', 'G'), frame_fps, Size(frame_width, frame_height), isColor);

  //TLD framework 

  //Read parameters file
  tld.read(fs.getFirstTopLevelNode());
  // Mat frame;
  Mat last_gray;
  Mat first;


  // if (min(box.width,box.height)<(int)fs.getFirstTopLevelNode()["min_win"])
  // {
      // cout << "Bounding box too small, try again." << endl;
      // gotBB = false;
  // }

  //Output file
  FILE  *bb_file = fopen("bounding_boxes.txt","w");



  //Run-time

  BoundingBox pbox;
  //搜索框
  BoundingBox box_1;
  vector<Point2f> pts1;
  vector<Point2f> pts2;
  bool status=true;
  int frames = 1;
  int detections = 1;

  // double angle = 45;//旋转角度 逆时针
  // double angle_xuan = -45;//旋转角度 顺时针
  // double scale = 1;//变换尺寸
  // Mat rotateMat;
  // Mat rotateMat_xuan;

  //搜索框位置 转换
    // box_1.x = 686 - SEARCH_W / 2;//558;
    // box_1.y = 174 - SEARCH_H / 2;//46;
    // box_1.width = SEARCH_W;
    // box_1.height = SEARCH_H;

  // 针对特定图像 的 搜索框 转换 跟踪框坐标
  // box.x = 686 - box_1.x - 16;//558;
  // box.y = 174 - box_1.y - 16;//46;
  // box.width = 32;
  // box.height = 32;
      //搜索框位置 转换
    //X= 636 Y = 1073
#define LIVE1 1
#ifdef LIVE1
    box.x = 666;
    box.y = 743;
    box.width = 80;
    box.height = 80;
#elif LIVE2
	box.x = 150;
	box.y = 1082;
	box.width = 80;
	box.height = 80;

#elif LIVE3
#endif

    box_1.x = box.x - SEARCH_W / 2;//558;
    box_1.y = box.y - SEARCH_H / 2;//46;
    box_1.width = SEARCH_W;
    box_1.height = SEARCH_H;
	
	box.x = box.x - box_1.x;//558;
    box.y = box.y - box_1.y;//46;

  if (min(box.width,box.height)<(int)fs.getFirstTopLevelNode()["min_win"])
  {
      cout << "Bounding box too small, try again." << endl;
//      gotBB = false;
      return 0;
  }
  pthread_t TLD_learn;
  pthread_create(&TLD_learn, NULL, tracking_learn_process, NULL);

  //创建RNG对象，使用默认种子“-1”
    RNG rng;

    int xg_num = 90;
    while(1)
    {
        //产生[0,1)范围内均匀分布的double类型数据
        // int rng_x = rng.uniform((int)0,(int)12) - 6;
        // int rng_y = rng.uniform((int)0,(int)12) - 6;
        // int rng_angel = rng.uniform((int)0,(int)2) - 1;

        double all_t=(double)getTickCount();

        // capture >> frame;
        string jpg_name = "./jpg/xg_" + to_string(xg_num) + ".jpg";
        printf("jpg_name:%s\n", jpg_name.c_str());
        if(xg_num > 488)
        {
            break;
        }
        xg_num++;
        Mat frame = imread(jpg_name, 1);
        // Point2f center = Point2f((frame.cols / 2) + rng_x, (frame.rows / 2) + rng_y);//旋转中心

        //逆时针旋转 把旋转的视频矫正为正常的视频，方便跟踪算法测试
        // rotateMat = getRotationMatrix2D(center, (angle * frames) + rng_angel, scale);
        // warpAffine(frame, rotateImg_temp, rotateMat, frame.size());

        //把矫正后的视频在顺时针 还原为原视频 保存
        // rotateMat_xuan = getRotationMatrix2D(center, angle_xuan * frames, scale);

        //从矫正的视频中扣出搜索区域
        // rotateImg = rotateImg_temp(box_1);
		rotateImg = frame(box_1);


        //第一帧为初始化帧
        if(frames == 1)
        {
            //在第一帧上画框
            cvtColor(rotateImg, last_gray, CV_RGB2GRAY);
            // drawBox(rotateImg,box);

            //保存 矫正后的第一帧视频到文件
            // writer.write(rotateImg_temp);

            //保存逆时针还原的第一帧视频到文件
            // warpAffine(rotateImg_temp, rotateImg_temp_xuan, rotateMat_xuan, frame.size());
            // writer1.write(rotateImg_temp_xuan);

            //          imwrite("box.bmp",rotateImg(box));
            //          imwrite("rotateImg.bmp",rotateImg);

            //tld初始化
            tld.init(last_gray,box,bb_file);
            //检测计数
            detections++;
            //帧计数
            frames++;
            continue;
        }

        //get frame
        cvtColor(rotateImg, current_gray, CV_RGB2GRAY);
        //Process Frame
        double t=(double)getTickCount();

        tld.processFrame(last_gray,current_gray,pts1,pts2,pbox,status,tl,bb_file);

        t=(double)getTickCount()-t;
        double use_time = t*1000/getTickFrequency();
        printf("*************************processFrame %gms\n", use_time);


        xg_box.x = box_1.x + pbox.x;
        xg_box.y = box_1.y + pbox.y;
        xg_box.width = pbox.width;
        xg_box.height = pbox.height;
        //Draw Points
        if (status)
        {
    //      drawPoints(rotateImg,pts1);
    //      drawPoints(rotateImg,pts2,Scalar(0,255,0));
            // drawBox(rotateImg,pbox);
            detections++;


            drawBox(frame,xg_box,Scalar(0, 0, 255), 2);
            //搜索框
            drawBox(frame,box_1,Scalar(255, 0, 0), 2);
        }


        swap(last_gray,current_gray);
        pts1.clear();
        pts2.clear();
        frames++;
        printf("Detection rate: %d/%d\n",detections,frames);




        if(frames > 1)
        {
            printf("pbox.x:%d  pbox.y:%d\n", pbox.x, pbox.y);
            if((pbox.x < S_SCALE) || (pbox.y < S_SCALE) || (SEARCH_H - (pbox.x + pbox.width) < S_SCALE) || (SEARCH_W - (pbox.y + pbox.height)) < S_SCALE)
            {
    //            printf("box.x:%d y:%d w:%d h:%d\n", pbox.x, pbox.y, pbox.width, pbox.height);
                box_1.x = pbox.x + pbox.width / 2 + box_1.x - SEARCH_W / 2;
                box_1.y = pbox.y + pbox.height / 2 + box_1.y - SEARCH_H / 2;
                box_1.width = SEARCH_W;
                box_1.height = SEARCH_H;
    //            printf("box_1.x:%d y:%d w:%d h:%d\n", box_1.x, box_1.y, box_1.width, box_1.height);
	
				  if(box_1.x < 1)
                  {
                      box_1.x = 1;
                  }

                  if(box_1.x > 2048 - SEARCH_W)
                  {
                      box_1.x = 2048 - SEARCH_W - 1;
                  }
				  
				  if(box_1.y < 1)
                  {
                      box_1.y = 1;
                  }
                  if(box_1.y > 2048 - SEARCH_H)
                  {
                      box_1.y = 2048 - SEARCH_H - 1;
                  }

                pbox.x = SEARCH_W / 2 - pbox.width / 2;
                pbox.y = SEARCH_H / 2 - pbox.height / 2;
    //            pbox.x = 112;
    //            pbox.y = 112;
            }
        }


        //定义要显示的字符
        string text = "[" + to_string(frames) + "]" + "time:" + to_string(use_time) + " ms";

        //在矫正了的视频上 逆时针旋转45°，叠加字符，最后保存，形成的视频流还是旋转的
        // warpAffine(rotateImg_temp, rotateImg_temp_xuan, rotateMat_xuan, frame.size());
        // putText(rotateImg_temp_xuan, text, Point(20,40) , FONT_HERSHEY_COMPLEX, 1, cv::Scalar(0, 0, 255), 1, 8, 0);
        // writer1.write(rotateImg_temp_xuan);

        //对矫正 的视频进行保存 ，该视频是正常的
        putText(frame, text, Point(20,120) , FONT_HERSHEY_COMPLEX, 4, cv::Scalar(0, 0, 255), 1, 8, 0);
        writer.write(frame);

//        all_t=(double)getTickCount()-all_t;
//        double all_use_time = all_t*1000/getTickFrequency();
//        printf("*************************all_use_time %gms\n", all_use_time);


    }


  if (rep)
  {
    rep = false;
    tl = false;
    fclose(bb_file);
    bb_file = fopen("final_detector.txt","w");

    capture.release();
    capture.open(video);

  }
  fclose(bb_file);
  return 0;
}
#endif
